#include <imp/imp_log.h>
#include <imp/imp_common.h>
#include <system/system.h>
#include <ivs_move.h>
#include <imp_move_filter.h>
#ifdef DMALLOC
#include <imp/dmalloc.h>
#endif
#define VERSION_SKIP_FRAME 1
#define CALC_SUM_DEBUG 0
#if CALC_SUM_DEBUG
IMPRect RoiPointer[16];
#endif
#define TAG  "IVS_MOVE"

typedef struct {
	int width;
	int height;
	int NumOfHistory;
	uint8_t** bufs;
	int index;

	int roiRectCnt;
	int isUseSimd;
	int skipFrameCnt;

	int id_new;
	int id_old;

	int* retForRoi;
	//v16u8 vsel;
	FilterEngine* filterEngine;
	uint8_t* frame_data;
} IVSMove_c;

extern inline int c_clip3(int v, int i_min, int i_max);

static int reset_param(IVSMove_c* ivsMove, int* _sense, IMPRect* _roiRect, int _roiRectCnt, int _width, int _height, IMP_IVS_MoveParam* param)
{
	int i = 0;

	ivsMove->roiRectCnt = c_clip3(_roiRectCnt, 0, IMP_IVS_MOVE_MAX_ROI_CNT);

	if( (_roiRect != NULL) && (_roiRectCnt > 0) && (_roiRectCnt <= IMP_IVS_MOVE_MAX_ROI_CNT) ){
		if(ivsMove->filterEngine != NULL)
			freeFilterEngine(ivsMove->filterEngine);
		zSize simg={_width>>1,_height>>1};

		IMPRect roiRect[IMP_IVS_MOVE_MAX_ROI_CNT];
		for (i = 0; i < ivsMove->roiRectCnt; i++) {
			roiRect[i].p0.x = c_clip3(_roiRect[i].p0.x, 0, _width - 1) >> 1;
			roiRect[i].p0.y = c_clip3(_roiRect[i].p0.y, 0, _height - 1) >> 1;
			roiRect[i].p1.x = c_clip3(_roiRect[i].p1.x, 0, _width - 1) >> 1;
			roiRect[i].p1.y = c_clip3(_roiRect[i].p1.y, 0, _height - 1) >> 1;
		}

		ivsMove->filterEngine = Init(simg, ivsMove->roiRectCnt,roiRect);
		if (ivsMove->filterEngine == NULL) {
			IMP_LOG_ERR(TAG, "%s(%d): Init ivsMove->filterEngine failed\n", __func__, __LINE__);
			return -1;
		}
		param->roiRectCnt = ivsMove->roiRectCnt;
		memcpy(param->roiRect, _roiRect, sizeof(roiRect));
	}

	for( i = 0; i < ivsMove->roiRectCnt; i++){
		switch(_sense[i]){
			case 0:
				ivsMove->filterEngine->Noise_Thresh[i] = 1365;
				break;
			case 1:
				ivsMove->filterEngine->Noise_Thresh[i] = 455;
				break;
			case 2:
				ivsMove->filterEngine->Noise_Thresh[i] = 151;
				break;
			case 3:
				ivsMove->filterEngine->Noise_Thresh[i] = 50;
				break;
			case 4:
				ivsMove->filterEngine->Noise_Thresh[i] = 16;
				break;
			//unstable sense below:
			case 5:
				ivsMove->filterEngine->Noise_Thresh[i] = 8;
				break;
			case 6:
				ivsMove->filterEngine->Noise_Thresh[i] = 4;
				break;
			case 7:
				ivsMove->filterEngine->Noise_Thresh[i] = 2;
				break;
			case 8:
				ivsMove->filterEngine->Noise_Thresh[i] = 1;
				break;

			default:
				IMP_LOG_ERR(TAG, "have none of this sense, the support range is 0-4\n");
				if(ivsMove->filterEngine != NULL)
					freeFilterEngine(ivsMove->filterEngine);
				goto SENSE_INVALID;
		}
	}
	memcpy(param->sense, _sense, sizeof(param->sense));
	return 0;
SENSE_INVALID:
	return -1;
}

#define width ivsMove->width
#define height ivsMove->height
#define NumOfHistory ivsMove->NumOfHistory
#define sense ivsMove->sense
#define NOISE_THRESH filterEngine->Noise_Thresh
#define bufs ivsMove->bufs
#define index ivsMove->index
#define roiRectCnt ivsMove->roiRectCnt
#define vsel ivsMove->vsel
#define isUseSimd ivsMove->isUseSimd
#define filterEngine ivsMove->filterEngine
#define retForRoi ivsMove->retForRoi
#define id_new ivsMove->id_new
#define id_old ivsMove->id_old
#define innerSkipFrameCnt ivsMove->skipFrameCnt
#define frame_data ivsMove->frame_data

IVSMove_c* IVSMove_init(int _width,int _height,int* _sense,int _skipFrameCnt, int _roiRectCnt, IMPRect* roiRect)
{
	int i = 0;
#if CALC_SUM_DEBUG
	for(i=0;i<_roiRectCnt;i++){
		RoiPointer[i].p0.x = roiRect[i].p0.x;
		RoiPointer[i].p0.y = roiRect[i].p0.y;
		RoiPointer[i].p1.x = roiRect[i].p1.x;
		RoiPointer[i].p1.y = roiRect[i].p1.y;
	}
#endif
	IVSMove_c* ivsMove = (IVSMove_c*)calloc(1, sizeof(IVSMove_c));
	if (NULL == ivsMove) {
		IMP_LOG_ERR(TAG, "malloc ivsMove failed\n");
		goto err_malloc_ivsMove;
	}

	width = _width;
	height = _height;

	NumOfHistory = 4;
	retForRoi = (int*)calloc(IMP_IVS_MOVE_MAX_ROI_CNT, sizeof(int));
	if (NULL == retForRoi){
		IMP_LOG_ERR(TAG, "retForRoi malloc retForRoi error\n");
		goto err_malloc_retForRoi;
	}

	innerSkipFrameCnt = _skipFrameCnt;
	index = 0;
	roiRectCnt = _roiRectCnt;
	id_new = 0;
	id_old = 0;

	isUseSimd = is_has_simd128();
	if (!!isUseSimd) {
        assert(0);
#if 0
		/* A key to MXUv2. */
		mk();
		uint8_t sel[16];
		for(i=0;i<8;i++){
			sel[i]=i*4;
			sel[i+8]=sel[i]+1;
		}
		LU1Q(vsel,sel,0);
#endif
	}
	bufs = (uint8_t**)calloc(NumOfHistory, sizeof(uint8_t*));
	if (NULL == bufs) {
		IMP_LOG_ERR(TAG, "malloc bufs error\n");
		goto err_malloc_bufs;
	}

	for(i = 0; i < NumOfHistory; i++){
		bufs[i] = (uint8_t*)calloc(width*height, sizeof(uint8_t));
		if (NULL == bufs[i]) {
			IMP_LOG_ERR(TAG, "malloc bufs[%d] error\n", i);
			goto err_malloc_buf;
		}
	}

	zSize simg={width,height};
	filterEngine = Init(simg,_roiRectCnt, roiRect);
	if (filterEngine == NULL) {
		IMP_LOG_ERR(TAG, "Init error\n");
		goto err_Init;
	}

	frame_data = (uint8_t*)calloc((width<<1)*(height<<1), sizeof(uint8_t));
	if (NULL==frame_data){
	  IMP_LOG_ERR(TAG, "calloc frame_data failed!\n");
	  goto err_calloc_frame_data;
	}

	for( i = 0; i < _roiRectCnt; i++){
		switch(_sense[i]){
			case 0:
				NOISE_THRESH[i] = 1365;
				break;
			case 1:
				NOISE_THRESH[i] = 455;
				break;
			case 2:
				NOISE_THRESH[i] = 151;
				break;
			case 3:
				NOISE_THRESH[i] = 50;
				break;
			case 4:
				NOISE_THRESH[i] = 16;
				break;
			//unstable sense below:
			case 5:
				NOISE_THRESH[i] = 8;
				break;
			case 6:
				NOISE_THRESH[i] = 4;
				break;
			case 7:
				NOISE_THRESH[i] = 2;
				break;
			case 8:
				NOISE_THRESH[i] = 1;
				break;

			default:
				IMP_LOG_ERR(TAG, "have none of this sense, the support range is 0-4\n");
				goto err_sense;
		}
	}

	return ivsMove;

err_sense:
	free(frame_data);
err_calloc_frame_data:
	freeFilterEngine(filterEngine);
err_Init:
	i = NumOfHistory;
err_malloc_buf:
	for(--i; i >= 0; i--){
		free(bufs[i]);
	}
	free(bufs);
err_malloc_bufs:
	free(retForRoi);
err_malloc_retForRoi:
	free(ivsMove);
err_malloc_ivsMove:
	return NULL;
}

void resize(IVSMove_c* ivsMove,uint8_t* in_data, uint8_t* out_data, int _width, int _height)
{
	int i, j;
	int old_r, old_c;
	int old_w = _width<<1;
	uint8_t *src, *dst;
	if (!!isUseSimd) {
        assert(0);
#if 0
		v16u8 v0, v1;
		for (j = 0, old_r = 0; j < _height;j++,old_r+=2) {
			src = in_data + old_r*old_w;
			dst = out_data + j*_width;
			for ( i = 0, old_c = 0; i < (_width-16) ; i+=16, old_c+=32) {
				LU1Q(v0,src+old_c,0);
				LU1Q(v1,src+old_c,16);
				SHUFV(v0,v1,v0,vsel);
				SU1Q(v0,dst+i,0);
			}
			for (; i < _width; i++) {
				dst[i] = src[i*2];
			}
		}
#endif
	} else {
		for (j = 0; j < _height; j++) {
			for (i = 0; i < _width; i++) {
				int oidx = i+j*_width;
				int iidx = i*2 + j*4*_width;
				out_data[oidx] = in_data[iidx];
			}
		}
	}
}

//#define TIME
int update_mhi( IVSMove_c* ivsMove, ivs_move_t* mv, uint8_t* data, IMP_IVS_MoveOutput* res)
{
	int ret = 0;
#if DEBUG_BUF
	static int frameCnt = 0;
#endif

#ifdef TIME
	struct timeval tv;
	gettimeofday(&tv, NULL);
	uint64_t time_last = tv.tv_sec*1000000 + tv.tv_usec;
#endif
	if(index < NumOfHistory-1){
		/* bufs[index] = data; */
		resize(ivsMove,data,bufs[index],width,height);
		index++;
		id_new = index;
		memset(res->retRoi, (int)(0), roiRectCnt*sizeof(int));
		/* return ; */
	}else{
		id_old = id_old % NumOfHistory;
		id_new = id_new % NumOfHistory;

		if(index >= innerSkipFrameCnt){
			resize(ivsMove,data,bufs[id_new],width,height);

			if(index >= innerSkipFrameCnt + NumOfHistory - 1){

				uint8_t* data1 = bufs[id_new];
				uint8_t* data2 = bufs[id_old];

				Image buf_idx1 = CreateImage(data1,height,width,0);
				Image buf_idx2 = CreateImage(data2,height,width,0);

				move_detect(buf_idx1,buf_idx2,buf_idx2, retForRoi, filterEngine, isUseSimd);

				innerSkipFrameCnt = mv->param.skipFrameCnt;

#if CALC_SUM_DEBUG
				printf("CALC_SUM_DEBUG:");
				int ct = 0, it = 0, et = 0;
				int sumcount[roiRectCnt];
				for(ct = 0;ct<roiRectCnt;ct++){
					sumcount[ct] = 0;
					IMPRect rect = RoiPointer[ct];
					printf("rect[%d,%d]--[%d,%d],   ",rect.p0.x,rect.p0.y,rect.p1.x,rect.p1.y);
					for(it = rect.p0.y ;it <= rect.p1.y; it++){
						uint8_t* data = buf_idx2.data + it*buf_idx2.step;
						for(et = rect.p0.x; et <= rect.p1.x; et++){
							sumcount[ct] += (data[et] > 0);
						}

					}

					printf("%d, ",sumcount[ct]);
				}
				printf("\n");
#endif
				index = NumOfHistory - 1;
				memcpy(res->retRoi, retForRoi, roiRectCnt*sizeof(int));
			}else{
				//memset(res->retRoi, (int)(-1), roiRectCnt*sizeof(int));
				index++;
				ret = 1;
			}

		}else{
			//memset(res->retRoi, (int)(-1), roiRectCnt*sizeof(int));
			index++;
			ret = 1;
		}

		id_old++;
		id_new++;
	}

//	memcpy(res->retRoi, retForRoi, roiRectCnt*sizeof(int));

#ifdef TIME
	gettimeofday(&tv, NULL);
	time_last = tv.tv_sec*1000000 + tv.tv_usec - time_last;
	uint64_t time_ms = time_last*1.0/1000;
	printf("last time : %llu us\n", time_last);
	IMP_LOG_ERR(TAG, "last time : %llu us\n", time_last);
#endif
	return ret;
}

#undef width
#undef height
#undef NumOfHistory
#undef sense
#undef NOISE_THRESH
#undef silh
#undef dst
#undef bufs
#undef index
#undef roiRectCnt
#undef vsel
#undef isUseSimd
#undef filterEngine
#undef innerSkipFrameCnt
#undef retForRoi
#undef id_new
#undef id_old
#undef frame_data

ivs_move_t* imp_alloc_move(IMP_IVS_MoveParam* param, int (*free_data)(void *data))
{
	int i = 0;
	int _roiRectCnt = c_clip3(param->roiRectCnt, 0, IMP_IVS_MOVE_MAX_ROI_CNT);
	ivs_move_t *move = (ivs_move_t *)calloc(1, sizeof(ivs_move_t));
	if (move == NULL) {
		IMP_LOG_ERR(TAG, "calloc ivs move failed\n");
		goto err_calloc_move;
	}

	memcpy(&move->param, param, sizeof(IMP_IVS_MoveParam));
	IMPRect _roiRect[IMP_IVS_MOVE_MAX_ROI_CNT];
	int _sense[IMP_IVS_MOVE_MAX_ROI_CNT];
	for (i = 0; i < _roiRectCnt; i++) {
		_roiRect[i].p0.x = c_clip3(move->param.roiRect[i].p0.x, 0, param->frameInfo.width - 1) >> 1;
		_roiRect[i].p0.y = c_clip3(move->param.roiRect[i].p0.y, 0, param->frameInfo.height - 1) >> 1;
		_roiRect[i].p1.x = c_clip3(move->param.roiRect[i].p1.x, 0, param->frameInfo.width - 1) >> 1;
		_roiRect[i].p1.y = c_clip3(move->param.roiRect[i].p1.y, 0, param->frameInfo.height - 1) >> 1;
		_sense[i] = c_clip3(move->param.sense[i], 0,  4);
		IMP_LOG_DBG(TAG, "move->param.sense[%d]=%d\n", i, move->param.sense[i]);
	}

	move->index = 0;

	move->free_data = free_data;

	/* move->ivs_move = IVSMove_init(param->frameInfo.width,param->frameInfo.height,param->sense,2); */
	move->ivs_move = IVSMove_init(move->param.frameInfo.width>>1,
								  move->param.frameInfo.height>>1,
								  _sense,
								  move->param.skipFrameCnt,
								  _roiRectCnt,
								  _roiRect);
	if(move->ivs_move==NULL) {
		IMP_LOG_ERR(TAG, "IVSMove_init failed\n");
		goto err_IVSMove_init;
	}

	return move;

err_IVSMove_init:
	free(move);
err_calloc_move:
	return NULL;
}

void imp_free_move(ivs_move_t *move)
{
	if(!move)
	  return;

	IVSMove_c* ivs_mv = ((IVSMove_c*)(move->ivs_move));

	if (ivs_mv->frame_data != NULL) {
		free(ivs_mv->frame_data);
		ivs_mv->frame_data = NULL;
	}

	if(ivs_mv->filterEngine != NULL)
		freeFilterEngine(ivs_mv->filterEngine);

	/* if (move->free_data){ */
	/* 	int i; */
	/* 	for(i =0; i < ivs_mv->NumOfHistory; i++){ */
	/* 		if(ivs_mv->bufs[i]){ */
	/* 			move->free_data(ivs_mv->bufs[i]); */
	/* 			ivs_mv->bufs[i] = NULL; */
	/* 		} */
	/* 	} */
	/* } */

	int i;
	for (i = 0; i < ivs_mv->NumOfHistory; i++) {
		if(ivs_mv->bufs[i]){
			free(ivs_mv->bufs[i]);
			ivs_mv->bufs[i] = NULL;
		}
	}

	if(ivs_mv->bufs){
		free(ivs_mv->bufs);
		ivs_mv->bufs = NULL;
	}

	if (ivs_mv->retForRoi) {
		free(ivs_mv->retForRoi);
	}

	free(ivs_mv);
	ivs_mv = NULL;

	free(move);
	move = NULL;
}

int imp_move_preprocess(ivs_move_t *move, IMPFrameInfo *frame)
{
  int size_of_frame = frame->width*frame->height;
  memcpy(((IVSMove_c*)(move->ivs_move))->frame_data, (void *)frame->virAddr, size_of_frame*sizeof(uint8_t));

  return 1;
}

int imp_move_process(ivs_move_t *move, IMPFrameInfo *frame, IMP_IVS_MoveOutput *result)
{
	int ret = 0;

	if(move == NULL) {
		IMP_LOG_ERR(TAG, "move is NULL !\n");
		goto err_move_is_null;
	}

	if (move->free_data){
		move->free_data((void *)(frame->virAddr));
	}

	if(result == NULL) {
		IMP_LOG_ERR(TAG, "result is NULL !\n");
		goto err_result_is_null;
	}
	if (!!(((IVSMove_c*)(move->ivs_move))->isUseSimd)) {
		/* Insert MXU key first. */
		mk();
	}

	ret = update_mhi((IVSMove_c*)move->ivs_move,move, ((IVSMove_c*)(move->ivs_move))->frame_data/*(uint8_t* )(frame->virAddr)*/, result);

	return ret;

err_result_is_null:
err_move_is_null:
	return -1;
}

int imp_get_move_param(ivs_move_t *move, IMP_IVS_MoveParam *param)
{
	if (!move) {
		IMP_LOG_ERR(TAG, "move is NULL\n");
		return -1;
	}
	memcpy(param, &move->param, sizeof(IMP_IVS_MoveParam));

	return 0;
}

int imp_set_move_param(ivs_move_t *move, IMP_IVS_MoveParam *param)
{
	if (!move || !param) {
		IMP_LOG_ERR(TAG, "move=%p, param=%p is NULL\n");
		return -1;
	}
	IVSMove_c* ivs_mv = ((IVSMove_c*)(move->ivs_move));

#if 0
	if (move->free_data){
		int i;
		for(i =0; i < ivs_mv->NumOfHistory; i++){
			if(ivs_mv->bufs[i]){
				move->free_data(ivs_mv->bufs[i]);
				ivs_mv->bufs[i] = NULL;
			}
		}
	}
#endif
	return reset_param(ivs_mv, param->sense, param->roiRect, param->roiRectCnt, move->param.frameInfo.width, move->param.frameInfo.height, &move->param);
}

int imp_flush_frame(ivs_move_t *move)
{
	return 0;
}
